#! /usr/bin/env python
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Image

class Match:
    def __init__(self):
        self.bridge = CvBridge()
        self.img_sub = rospy.Subscriber('/camera/rgb/image_color',Image,self.imgCallback)
        self.img_pub = rospy.Publisher('/camera/rgb/image_color_match',Image,queue_size=1)
        self.template = cv2.imread("0.jpg")
        h1, w1 = self.template.shape[:2]
        print("h1={0}, w1={0}".format(h1, w1))
        self.template = cv2.resize(self.template, (int(h1*1), int(w1*1)))
        # self.match(1)

    def imgCallback(self,data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data,'bgr8')
            # cv2.imshow('src',cv_image)
            # cv2.waitKey(1)
            self.match(cv_image)

        except e:
            print(e)


    def match(self, data):
        
        img = data
        # img = cv2.imread("1.jpg")
        cv2.imshow("template", self.template)
        # h2, w2 = img.shape[:2]

        # print("h1={0}, w1={0}".format(h1, w1))
        # print('h2={0}, w2={0}'.format(h2, w2))

        result = cv2.matchTemplate(img, self.template, cv2.TM_CCOEFF_NORMED)
        min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(result)
        top_left = max_loc
        bottom_right = (top_left[0] + self.template.shape[1], top_left[1] + self.template.shape[0])
        cv2.rectangle(img, top_left, bottom_right, (255, 0, 0), 2)
        # cv2.imshow("img", img)
        imgmsg = self.bridge.cv2_to_imgmsg(img, "bgr8")
        self.img_pub.publish(imgmsg)

        # image_m = self.bridge.imgmsg_to_cv2(imgmsg, "bgr8")
        # cv2.imshow("img", image_m)
        # cv2.waitKey(0)



def main():
    rospy.init_node('take_photo',anonymous = False)
    match = Match()
    rospy.spin()

if __name__ == "__main__":
    try:
        main()
    except rospy.ROSInterruptException:
        pass